#include "v4l2.hpp"
/*
IOCTL:
VIDIOC_REQBUFS：分配内存
VIDIOC_QUERYBUF：把VIDIOC_REQBUFS中分配的数据缓存转换成物理地址
VIDIOC_QUERYCAP：查询驱动功能
VIDIOC_ENUM_FMT：获取当前驱动支持的视频格式
VIDIOC_S_FMT：设置当前驱动的频捕获格式
VIDIOC_G_FMT：读取当前驱动的频捕获格式
VIDIOC_TRY_FMT：验证当前驱动的显示格式
VIDIOC_CROPCAP：查询驱动的修剪能力
VIDIOC_S_CROP：设置视频信号的边框
VIDIOC_G_CROP：读取视频信号的边框
VIDIOC_QBUF：把数据从缓存中读取出来
VIDIOC_DQBUF：把数据放回缓存队列
VIDIOC_STREAMON：开始视频显示函数
VIDIOC_STREAMOFF：结束视频显示函数
VIDIOC_QUERYSTD：检查当前视频设备支持的标准，例如PAL或NTSC
*/

ManV4L2::ManV4L2(string name , v4l2_buf_type type){
	m_name = name;
	m_video_buf_type = type;
    	memset(m_buffers, 0, sizeof(ManBuffer) * BUFFER_COUNT);
	m_stream_on = false;
}

int ManV4L2::init(){
	m_fd = open(m_name.c_str(),O_RDWR);
	
	if(m_fd < 0){
		cout << "Camera open failed!: " << m_name << endl;
		return -1;
	}

	if(ioctl(m_fd,VIDIOC_QUERYCAP,&m_cap) < 0){
		cout << "VIDIOC_QUERYCAP failed!" << endl;
		return -1;
	}
	if(m_video_buf_type == MAN_CAPTURE_MPLANE || m_video_buf_type == MAN_CAPTURE){
		if(m_cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)
			m_video_buf_type = MAN_CAPTURE;
		else
			m_video_buf_type = MAN_CAPTURE_MPLANE;

	}
	return 0;
}

void ManV4L2::printCameraInfo(){
	cout << "INFO >>>>>>>>>>>>>>>>>> " << endl;
	cout << "CAMERA CAP: " << endl;
	cout << "Driver: " << m_cap.driver << endl;
	cout << "Card: " << m_cap.card << endl;
	cout << "Bus: " << m_cap.bus_info << endl;
	cout << "Version: " << ((m_cap.version>>16)&&0xff) << "." << ((m_cap.version>>24)&&0xff) << endl;
	cout << "Capabilities: " << hex << m_cap.capabilities << dec << endl;
	int size = sizeof(man_videocap) / sizeof(man_videocap[0]);
        for(int i = 0; i < size;i++){
		if((m_cap.capabilities & man_videocap[i].cap) != 0){
			cout << man_videocap[i].name << ": " << "yes" << endl;
		} else {
			cout << man_videocap[i].name << ": " << "no" << endl;
		}
	}
	
	cout << endl;
	cout << endl;

	cout << "CAMERA FMT: " << endl;
	getFmt();
	cout << "Width: " << fmt.fmt.pix.width << endl;
	cout << "Height: " << fmt.fmt.pix.height << endl;
	unsigned tfmt = fmt.fmt.pix.pixelformat;
	cout << "FMT: ";
	for(int i = 0; i < 4; i++){
		cout.put((char)((tfmt >> (i * 8)) & 0xff));
	}

	cout << endl;
	cout << endl;

	cout << "INFO <<<<<<<<<<<<<<<<<< " << endl;
	return;
}


int ManV4L2::setFmt(){
	int ret = 0;
	ret = getFmt();
	unsigned tfmt = fmt.fmt.pix.pixelformat;
	string _fmt = "";
	for(int i = 0; i < 4; i++){
		_fmt = _fmt + ((char)((tfmt >> (i * 8)) & 0xff));
	}
	ret = setFmt(_fmt,fmt.fmt.pix.width,fmt.fmt.pix.height);
	return ret;
}

int ManV4L2::setFmt(string tfmt, unsigned int width, unsigned int height){
    
    memset(&fmt,0,sizeof(fmt));
    fmt.type = m_video_buf_type;
    fmt.fmt.pix.width = width;
    fmt.fmt.pix.height = height;
    if(tfmt.length() != 4){
	    cout << "FMT ERR: " << tfmt << endl;
	    return -1;
    }
    char r[4];
    for(int i = 0; i < 4 ; i++)
	    r[i] = tfmt[i];

    fmt.fmt.pix.pixelformat = v4l2_fourcc(r[0],r[1],r[2],r[3]);// 像素格式
    fmt.fmt.pix.field = V4L2_FIELD_NONE;// 逐行扫描，也有隔行扫描需要摄像头支持，这里了解不深大部分的摄像头都支持逐行扫描，默认此配置
    

    if(ioctl(m_fd, VIDIOC_TRY_FMT, &fmt) < 0)
    {
	cout << "VIDIOC_TRY_FMT failed: fmt(" << tfmt + ") " << "width(" << width << ") " << "height(" << height << ")" << endl;
    	return -1;
    }

    if(ioctl(m_fd,VIDIOC_S_FMT,&fmt) < 0)
    {
    	cout << "VIDIOC_S_FMT failed!" << endl;
    	return -1;
    }
  
    for(int i = 0; i < 4; i++){
	    if((char)((fmt.fmt.pix.pixelformat >> (i * 8)) & 0xff) != r[i]){
		    cout << "Camera not support fmt: " << tfmt << "Cur fmt: ";
		    for(int j = 0; j < 4; j++){
			    cout.put((char)((fmt.fmt.pix.pixelformat >> (j * 8)) & 0xff));
		    }
		    cout << endl;
		    return -1;
	    }
    }

    //让内核申请内存
    struct v4l2_requestbuffers reqbuf;
    memset(&reqbuf, 0, sizeof(reqbuf));
    reqbuf.count = BUFFER_COUNT;// 个 buff 大小的缓存
    reqbuf.type = m_video_buf_type;
    reqbuf.memory = V4L2_MEMORY_MMAP; //内存映射

    if(ioctl(m_fd,VIDIOC_REQBUFS,&reqbuf) < 0)
    {
	    cout << "VIDIOC_REQBUFS failed!" << endl;
	    return -1;
    }
    struct v4l2_buffer buf;
    struct v4l2_plane* planes_buffer;
    struct ManPlaneStart* plane_start;
    m_num_planes = fmt.fmt.pix_mp.num_planes;
    
    for(int i = 0;i < reqbuf.count;i ++)
    {
	    if(m_video_buf_type == MAN_CAPTURE_MPLANE) {
		    planes_buffer = (v4l2_plane *)calloc(m_num_planes, sizeof(*planes_buffer));
		    plane_start = (ManPlaneStart *)calloc(m_num_planes, sizeof(*plane_start));
		    memset(planes_buffer, 0, sizeof(*planes_buffer) * m_num_planes);
		    buf.m.planes = planes_buffer;
		    buf.length = m_num_planes;
	    }
	    buf.index = i;
	    buf.type = m_video_buf_type;
	    buf.memory = V4L2_MEMORY_MMAP;
	    if(ioctl(m_fd,VIDIOC_QUERYBUF,&buf) < 0)
	    {
		    cout << "VIDIOC_QUERYBUF failed!" << endl;
		    return -1;
	    }
	    
	    if(m_video_buf_type == MAN_CAPTURE_MPLANE) {
		    (m_buffers + i)->planes_buffer = planes_buffer;
		    (m_buffers + i)->plane_start = plane_start;
		    for(int j = 0; j < m_num_planes; j++) {
			    //printf("plane[%d]: length = %d\n", j, (planes_buffer + j)->length);
			    //printf("plane[%d]: offset = %d\n", j, (planes_buffer + j)->m.mem_offset);
			    (plane_start + j)->start = mmap (NULL /* start anywhere */,
					    (planes_buffer + j)->length,
					    PROT_READ | PROT_WRITE /* required */,
					    MAP_SHARED /* recommended */,
					    m_fd,
					    (planes_buffer + j)->m.mem_offset);
			    if (MAP_FAILED == (plane_start +j)->start) {
			    	    cout << "MMAP failed!" << endl;
				    return -1;
			    }
		    }
	    } else {
		    m_framebuf[i].length = buf.length;
		    m_framebuf[i].start = mmap(NULL,buf.length,
				    PROT_READ | PROT_WRITE,
				    MAP_SHARED,m_fd,buf.m.offset);

		    if(m_framebuf[i].start == MAP_FAILED)
		    {
			    cout << "MMAP failed!" << endl;
			    return -1;
		    }
	    }
	    
	    //让 buff 加入缓存区
	    if(ioctl(m_fd,VIDIOC_QBUF,&buf) < 0)
	    {
		    cout << "VIDIOC_QBUF failed!" << endl;
		    return -1;
	    }
    }
    
    return 0;
}

int ManV4L2::getFmt(){
	memset(&fmt,0,sizeof(fmt));
	fmt.type = m_video_buf_type;
	
	if(m_cap.capabilities & V4L2_CAP_EXT_PIX_FORMAT)
		fmt.fmt.pix.priv = V4L2_PIX_FMT_PRIV_MAGIC;
	if(ioctl(m_fd,VIDIOC_G_FMT,&fmt) < 0)
	{
		cout << "VIDIOC_G_FMT failed!" << endl;
		return -1;
	}

	return 0;
}

int ManV4L2::streamOff(){
	if(m_stream_on == true){
		enum v4l2_buf_type type = m_video_buf_type;
		if(ioctl(m_fd,VIDIOC_STREAMOFF,&type) < 0)
		{
			cout << "VIDIOC_STREAMOFF failed!" << endl;
			return -1;
		}
	}

	return 0;
}

int ManV4L2::putFrame(void * frame){
	volatile struct v4l2_buffer buf;
	buf.type = m_video_buf_type;
	buf.memory = V4L2_MEMORY_MMAP;
	buf.length = m_num_planes;
	volatile int i = 0;
	int retry = 0;
	
	for (i = 0; i < BUFFER_COUNT; i++){
		if(m_video_buf_type == MAN_CAPTURE_MPLANE) { 
			
			if(frame == m_buffers[i].plane_start->start){
				buf.m.planes = m_buffers[i].planes_buffer;
				buf.length = m_num_planes;
				buf.index = i;
				break;
			}

		} else {
			if(frame ==  m_framebuf[i].start){
				buf.index = i;
				break;
			}
		}
	}

	if(i == BUFFER_COUNT)
	{
		perror("VIDIOC_QBUF addr failed!\n");
		return -1;
	}

	int time = 10;
again:
	if(ioctl(m_fd,VIDIOC_QBUF,&buf) < 0)
	{
		if(retry < time || time == -1){
			retry++;
			usleep(1000);
			goto again;
		}
		perror("VIDIOC_QBUF failed!\n");
		return -1;
	}

	
	return 0;
}

void * ManV4L2::getFrame(int time){
	struct v4l2_buffer buf;
	int retry = 0;
	
	memset(&buf, 0, sizeof(buf));
	
	buf.type = m_video_buf_type;
	buf.memory = V4L2_MEMORY_MMAP;

	struct v4l2_plane tmp_plane;
	
	if(m_video_buf_type == MAN_CAPTURE_MPLANE) {
		buf.m.planes = &tmp_plane;
		buf.length = m_num_planes;
	}
	
again:
	if(ioctl(m_fd,VIDIOC_DQBUF,&buf) < 0) //取出buff
	{
		if(retry < time || time == -1){
			retry++;
			usleep(1000);
			goto again;
		}

		cout << "VIDIOC_DQBUF failed!" << endl;
		return NULL;
	}
	
	if(m_video_buf_type == MAN_CAPTURE_MPLANE) { 
		return m_buffers[buf.index].plane_start[0].start; //TODO only support num plane = 1
	} else {
		return m_framebuf[buf.index].start;
	}

}

int ManV4L2::streamOn(){
	if(m_stream_on == false) {

		//start camera capture
		enum v4l2_buf_type type = m_video_buf_type;
		if(ioctl(m_fd,VIDIOC_STREAMON,&type) < 0)
		{
			cout << "VIDIOC_STREAMON failed!" << endl;
			return -1;
		}
	}

	return 0;

}

int ManV4L2::getFmtSupport(){
	struct v4l2_fmtdesc vfmt;
	struct v4l2_frmsizeenum frmsize;
	struct v4l2_frmivalenum frmival;
	memset(&vfmt,0,sizeof(vfmt));
	memset(&frmsize,0,sizeof(frmsize));
	memset(&frmival,0,sizeof(frmival));
	vfmt.type = m_video_buf_type;
	cout << "Camera FMT ENUM:" << endl;
	for(int i = 0;;i++){
		vfmt.index = i;
		int ret = ioctl(m_fd,VIDIOC_ENUM_FMT,&vfmt);
		if (ret < 0)
		{
			cout << "=======================" << endl;
			break;
		}
		cout << "=======================" << endl;
		printf("index=%d\r\n",vfmt.index);
		printf("flags=%d\r\n",vfmt.flags);
		printf("discription=%s\r\n",vfmt.description);
		unsigned char *p = (unsigned char *)&vfmt.pixelformat;
		printf("pixelformat=%c%c%c%c\r\n",p[0],p[1],p[2],p[3]);
		printf("reserved=%d\r\n",vfmt.reserved[0]);
		
		cout << endl;
		frmsize.pixel_format = vfmt.pixelformat;
		frmival.pixel_format = frmsize.pixel_format;
		for(int j = 0;;j++){
			frmsize.index = j;
			int ret = ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &frmsize);
			if(ret < 0)
				break;

			if(frmsize.type == V4L2_FRMSIZE_TYPE_DISCRETE)
			{
				printf("DISCRETE: line-%d %d: {%d*%d}\r\n", __LINE__,
						frmsize.index,frmsize.discrete.width,
						frmsize.discrete.height);
			}

			else if(frmsize.type == V4L2_FRMSIZE_TYPE_STEPWISE)
			{
				printf("STEPWISE: line: %d %d: {%d*%d}\r\n", __LINE__,
						frmsize.index,frmsize.stepwise.max_width,
						frmsize.stepwise.max_height);	   
			}
			frmival.width = frmsize.discrete.width;
			frmival.height = frmsize.discrete.height;
			for(int k = 0;;k++){
				frmival.index = k;
				int ret = ioctl(m_fd, VIDIOC_ENUM_FRAMEINTERVALS, &frmival);
				if(ret < 0)
					break;

				printf("-%d.frameinterval:%d/%d\r\n", frmival.index,
						frmival.discrete.numerator, frmival.discrete.denominator);
			}
		}
	}
	return 0;

}

ManV4L2::~ManV4L2(){
	if(m_stream_on == true)
		streamOff();
	for(int i = 0; i < BUFFER_COUNT; i++){
		if(m_buffers[i].plane_start){
			for(int j = 0; j < m_num_planes; j++) {
				if(m_buffers[i].plane_start[j].start){
					munmap(m_buffers[i].plane_start[j].start, m_buffers[i].planes_buffer[j].length);
				}
			}
		}
		delete m_buffers[i].plane_start;
		delete m_buffers[i].planes_buffer;
	}
	if(m_fd > 0)
		close(m_fd);
	return;
}
